#include "renderthread.h"
#include "bilateral_normal_filter.h"
#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>
#include <QMessageBox>
#include <QDebug>

using namespace mtl;
using namespace itl;

RenderThread::RenderThread(QObject *parent) :
    QThread(parent)
{
    Stop = false;
    Ite = 0;
    MaxIte = 0;
    similarity = 0.0;
    ErrorCode=tr("No Error");
    hasData = false;
    isKeepShuffle = false;
    isExplicit = false;
    OutputPerIter = false;
    log.open("log.txt");


}

void RenderThread::setStart(bool state)
{
     mutex.lock();
    if (state)
    {
        Stop = false;

    }else
        Stop = true;
     mutex.unlock();
}

void RenderThread::threadReset()
{
    mutex.lock();
    Stop = false;
    Ite = 0;
    MaxIte = 0;
//    ErrorCode = tr("No Error");
    isKeepShuffle = false;
    mutex.unlock();

}

void RenderThread::ClearData()
{
    mutex.lock();
    Stop = false;
    Ite = 0;
    MaxIte = 0;
//    ErrorCode = tr("No Error");
    isKeepShuffle = false;
    vTime.clear();
    shuffle.clear();
    mutex.unlock();
    Joints.clear();

}


void RenderThread::getParameters(Parameter const *parameters, MPolygon inipoly,
                                 std::vector<MPolygon> & polygons, int method)
{


    mutex.lock();

    this->method = method;
    this->Polygons = polygons;

    this->iniPolygon = inipoly;
    this->MaxIte = parameters->MaxIte;
    this->similarity = parameters->similarity;
    this->TimeStep = parameters->TimeStep;
    this->hasData = true;
    this->isExplicit = parameters->isExplicit;
    this->Jump = parameters->Jump;
    this->VarNormal = parameters->VarNormal;
    this->VarVertex = parameters->VarVertex;

    if(!isKeepShuffle){
        shuffle.resize(this->iniPolygon.points.size(), 0);
        for(int i = 0; i < shuffle.size(); i++)
            shuffle[i] = i;
    }
    mutex.unlock();


    start();
}

void RenderThread::run()
{
    if(this->method == TotalArea){
        vTime.resize(this->Polygons[0].PointNo(), 0);
    }
    while (Ite < MaxIte)
    {
         if(Stop)
             return;
         mutex.lock();
         this->Ite++;
         if(this->OutputPerIter == true)
             OutputPolygon(this->Polygons, Ite);

         log<<"Iteration : "<<Ite<<"    ";
         log<<"Method : "<<this->Polygons[0].normals.size()<<"\n";
         switch(this->method){
             case ArcLen:
                for(int k = 0; k < Polygons.size();k++){
                    if(!arc_length(&this->iniPolygon,
                       &Polygons[k],
                       similarity,
                       TimeStep)){
                        Stop = true;
                        mutex.unlock();
                        emit resetButton();
                        return;
                    }
                }
                //kill time
                for(int i = 0; i < 30000; i++)
                {
                   if(Stop == true)
                   {
                       mutex.unlock();
                       emit resetButton();
                      return;
                  }else{
                   double tt = exp((double)i/7.0);
                   tt++;
                  }

                }
                 break;
             case ArcLenSqrt:
                 for(int k = 0; k < Polygons.size();k++){
                    if(!arc_length_sqrt(&this->iniPolygon,
                                        &this->Polygons[k],
                                        similarity,
                                        TimeStep)){
                        Stop = true;
                        mutex.unlock();
                        emit resetButton();
                        return;
                    }
                  }
                 break;
             case ArcLenFour:
                 for(int k = 0; k < Polygons.size();k++){
                    if(!arc_length_four(&this->iniPolygon,
                                        &this->Polygons[k],
                                        similarity,
                                        TimeStep)){
                        Stop = true;
                        mutex.unlock();
                        emit resetButton();
                        return;
                    }
                 }
                 break;
             case Area:
                 for(int k = 0; k < Polygons.size();k++){
                    if(!local_area(&this->iniPolygon,
                                   &this->Polygons[k],
                                   similarity,
                                    TimeStep)){
                        Stop = true;
                        mutex.unlock();
                        emit resetButton();
                    return;
                    }
                 }
                 //kill time
                 for(int i = 0; i < 50000; i++)
                 {
                    if(Stop == true)
                    {
                        mutex.unlock();
                        emit resetButton();
                       return;
                   }else{
                    double tt = exp((double)i/7.0);
                    tt++;
                   }

                 }
                 break;
             case AreaAbs:
                 for(int k = 0; k < Polygons.size();k++){
                    if(!local_area_abs(&this->iniPolygon,
                                       &this->Polygons[k],
                                        similarity,
                                        TimeStep)){
                        Stop = true;
                        mutex.unlock();
                        emit resetButton();
                        return;
                    }
                 }
                 //kill time
                 for(int i = 0; i < 50000; i++)
                 {
                    if(Stop == true)
                    {
                        mutex.unlock();
                        emit resetButton();
                       return;
                   }else{
                    double tt = exp((double)i/7.0);
                    tt++;
                   }

                 }
                 break;
             case AreaSquare:
                 for(int k = 0; k < Polygons.size();k++){
                    if(!local_area_square(&this->iniPolygon,
                                          &this->Polygons[k],
                                          similarity,
                                          TimeStep)){
                        Stop = true;
                        mutex.unlock();
                        emit resetButton();
                        return;
                    }
                 }
                 //kill time
                 for(int i = 0; i < 50000; i++)
                 {
                    if(Stop == true)
                    {
                        mutex.unlock();
                        emit resetButton();
                       return;
                   }else{
                    double tt = exp((double)i/7.0);
                    tt++;
                   }

                 }
                 break;
             case AreaMulti:
                 for(int k = 0; k < Polygons.size();k++){
                    if(!multiple_local_area(&this->iniPolygon,
                                            &this->Polygons[k],
                                            this->Jump,
                                            similarity,
                                            TimeStep)){
                        Stop = true;
                        mutex.unlock();
                        emit resetButton();
                        return;
                    }
                 }


                 break;
             case BilateralNormal:
                 for(int k = 0; k < Polygons.size();k++){
                    if(!bilateral_normal(&this->iniPolygon,
                                         &this->Polygons[k],
                                         this->similarity,
                                         this->TimeStep,
                                         this->VarVertex,
                                         this->VarNormal)){
                        Stop = true;
                        mutex.unlock();
                        emit resetButton();
                        return;
                    }
                 }

                 break;
             case COS_PI:
                 for(int k = 0; k < Polygons.size();k++){
                    if(!cos_pi(&this->iniPolygon,
                               &this->Polygons[k],
                               this->similarity,
                               this->TimeStep)){
                        Stop = true;
                        mutex.unlock();
                        emit resetButton();
                        return;
                    }
                 }

                 //kill time
                 for(int i = 0; i < 50000; i++)
                 {
                    if(Stop == true)
                    {
                        mutex.unlock();
                        emit resetButton();
                       return;
                   }else{
                    double tt = exp((double)i/7.0);
                    tt++;
                   }

                 }
                 break;
             case TotalArea:
                 if(!total_area(&this->iniPolygon,
                                this->Polygons,
                                this->similarity,
                                this->TimeStep)){
                    Stop = true;
                    mutex.unlock();
                    emit resetButton();
                   return;
                 }

                 //kill time
                 for(int i = 0; i < 50000; i++)
                 {
                    if(Stop == true)
                    {
                        mutex.unlock();
                        emit resetButton();
                       return;
                   }else{
                    double tt = exp((double)i/7.0);
                    tt++;
                   }

                 }
                 break;
             case TurningAngle:
                 for(int k = 0; k < Polygons.size();k++){
                    if(!turning_angle(&this->iniPolygon,
                                      &this->Polygons[k],
                                      this->similarity,
                                      this->TimeStep)){
                        Stop = true;
                        mutex.unlock();
                        emit resetButton();
                        return;
                    }

                 //kill time
                    for(int i = 0; i < 50000; i++){
                        if(Stop == true){
                            mutex.unlock();
                            emit resetButton();
                            return;
                        }else{
                            double tt = exp((double)i/7.0);
                            tt++;
                        }
                    }
                 }
                 break;
             case DiscreteCurvatureNormal:
                 for(int k = 0; k < Polygons.size();k++){
                    if(!discrete_curvature_normal(&this->iniPolygon,
                                                  &this->Polygons[k],
                                                  this->similarity,
                                                  this->TimeStep)){
                        Stop = true;
                        mutex.unlock();
                        emit resetButton();
                        return;
                    }

                 //kill time
                    for(int i = 0; i < 50000; i++){
                        if(Stop == true){
                            mutex.unlock();
                            emit resetButton();
                            return;
                        }else{
                            double tt = exp((double)i/7.0);
                            tt++;
                        }
                    }
                 }
                 break;
            default:
                break;
           }


         mutex.unlock();

//         qDebug() << "Scaling image in thread" << QThread::currentThread();
         //emit evolvePolygon(this->Polygons);
         for(int k = 0; k < this->Polygons.size(); k++)
            emit evolvePolygon(Polygons[k], k);
    }

    emit resetButton();

}



void RenderThread::OutputPolygon(std::vector<MPolygon> &polygons, int num){
    QString fstr = this->outputFile;
    QString ext = ".txt";
    int pos = fstr.lastIndexOf(ext);
    QString fix;

    QString fname = fstr.left(pos)+ "_" + fix.setNum(num)+ext;
    log<<fstr.toStdString()<<"\n";
    std::ofstream ofile(fname.toStdString().c_str());
    ofile.precision(12);
    ofile.setf(std::ios::fixed,std::ios::floatfield);

    for(int k = 0; k < polygons.size(); k++){
        for(int i = 0 ; i < polygons[k].PointNo(); i++)
            ofile<<polygons[k].points[i].x<<" "<<polygons[k].points[i].y<<"\n";
        ofile<<"\n";
    }
}

void RenderThread::remesh(MPolygon *apoly, double criteria)
{



    std::vector<int> NewCard;
    log.precision(12);
    log.setf(std::ios::fixed,std::ios::floatfield);
    do{
        int last = apoly->PointNo();
        MPolygon newpoly;
//        log<<"Before remesh\n";
        for(int i = 0; i< last; i++)
        {
            double xs = apoly->points[i].x -
                        apoly->points[(i+1)%last].x;
            double ys = apoly->points[i].y -
                        apoly->points[(i+1)%last].y;
            double len = std::sqrt(xs*xs + ys*ys);

//        log<<i<<" b: "<<apoly->points[i].x<<"  "<<apoly->points[i].y<<"\n";

            if(len >= criteria){
                newpoly.points.push_back(apoly->points[i]);
                NewCard.push_back(shuffle[i]);
            }else{
                log<<i<<" : "<<apoly->points[i].x<<" "<<apoly->points[i].y<<" ";
                log<<apoly->points[(i+1)%last].x<<" "<<apoly->points[(i+1)%last].y<<" "<<len<<"\n";
            }
        }

        if(apoly->points.size() == newpoly.points.size())
            break;

        log<<"remesh is needed\n";


        log<<"Remesh - new polygon:"<<apoly->PointNo()<<"\n";


        apoly->points.resize(newpoly.PointNo(), Point2d(0.0, 0.0));
        for(int i = 0; i< apoly->PointNo(); i++)
        {
            apoly->points[i].x = newpoly.points[i].x;
            apoly->points[i].y = newpoly.points[i].y;
//        log<<i<<" o: "<<apoly->points[i].x<<"  "<<apoly->points[i].y<<"\n";
        }

        isKeepShuffle = true;
        shuffle.resize(apoly->PointNo(), 0);
        for(int i = 0; i < apoly->PointNo(); i++){

            shuffle[i] = NewCard[i];
        }
    }while(1);
}

bool RenderThread::arc_length(MPolygon const *inipoly,
                              MPolygon *apoly,
                              double similarity, double TimeStep)
{


    if(!apoly)
        return false;



    MPolygon *poly = new MPolygon;

    if(!poly)
        return false;


    for(int i = 0; i<apoly->PointNo(); i++){
        poly->points.push_back(apoly->points[i]);
    }


    int last = apoly->PointNo();


    compressed2D<double>                  A(last, last);
    A = 0.0;

        // Create inserter for matrix m

    mtl::matrix::inserter<compressed2D<double> > *ins;
    ins = new mtl::matrix::inserter<compressed2D<double> >(A);

    dense_vector<double> newx(last, 1.0), newy(last, 1.0),
                         bx(last), by(last);

        // Insert value in
    for(int i = 0; i < last; i++){
            (*ins)[i][i] << 1.0+2*TimeStep + 2*TimeStep * similarity;
            (*ins)[i][(i+1)%last] << -2*TimeStep;
            bx[i] = apoly->points[i].x +
                    2*TimeStep * similarity * inipoly->points[i].x;
            by[i] = apoly->points[i].y +
                    2*TimeStep * similarity * inipoly->points[i].y;
     }
     delete ins;

     pc::ilu_0<compressed2D<double> >        P(A);

        // Termination criterion: r < 1e-6 * b or N iterations
     basic_iteration<double>       iter1(bx, 500, 1.e-5);
     basic_iteration<double>       iter2(by, 500, 1.e-5);
        // Solve Ax == b with left preconditioner P

     bicgstab(A, newx, bx, P, iter1);
     bicgstab(A, newy, by, P, iter2);

     if(!iter1.is_converged() ||
        !iter2.is_converged())
     {
         std::cout<<"Not converged\n";
         return false;
     }

     for(int i = 0; i < last; i++){
        apoly->points[i].x = newx[i];
        apoly->points[i].y = newy[i];
     }


//    this->draw_polygon(apoly);
//    std::cout<<"Arc-lenght\n";
    delete poly;

    return true;

}

bool RenderThread::arc_length_sqrt(MPolygon const *inipoly,
                                   MPolygon *apoly,
                                   double similarity, double TimeStep)
{


    if(!apoly)
        return false;


    int last = apoly->PointNo();

    double len[apoly->PointNo()];
    double tot_len = 0.0;
    for(int i = 0; i<apoly->PointNo(); i++){
        double xs = apoly->points[(i+1)%last].x -
                    apoly->points[i].x;

        double ys = apoly->points[(i+1)%last].y -
                    apoly->points[i].y;

        len[i] = std::sqrt(xs*xs + ys*ys);
        tot_len += len[i];
        if(len[i] < 1e-5)
            remesh(apoly, 1e-5);
    }
    if(tot_len < 1e-3)
        return false;
    if(apoly->PointNo() < 3)
    {
        std::cout<<"Not a Polygon anymore\n";
        return false;
    }

    last = apoly->PointNo();


    compressed2D<double>                  A(last, last);
    A = 0.0;

        // Create inserter for matrix m

    mtl::matrix::inserter<compressed2D<double> > *ins;
    ins = new mtl::matrix::inserter<compressed2D<double> >(A);

    dense_vector<double> newx(last, 1.0), newy(last, 1.0),
                         bx(last), by(last);

        // Insert value in
    for(int i = 0; i < last; i++){
            (*ins)[i][i] << 1.0+(1.0/len[i] + 2 * similarity) * TimeStep;
            (*ins)[i][(i+1)%last] << -TimeStep/len[i];

            bx[i] = apoly->points[i].x +
                    2*TimeStep * similarity * inipoly->points[i].x;
            by[i] = apoly->points[i].y +
                    2*TimeStep * similarity * inipoly->points[i].y;
     }
     delete ins;

     pc::ilu_0<compressed2D<double> >        P(A);

        // Termination criterion: r < 1e-6 * b or N iterations
     basic_iteration<double>       iter1(bx, 500, 1.e-5);
     basic_iteration<double>       iter2(by, 500, 1.e-5);
        // Solve Ax == b with left preconditioner P

     bicgstab(A, newx, bx, P, iter1);
     bicgstab(A, newy, by, P, iter2);

     if(!iter1.is_converged() ||
        !iter2.is_converged())
     {
         std::cout<<"Not converged\n";
         return false;
     }

     for(int i = 0; i < last; i++){
        apoly->points[i].x = newx[i];
        apoly->points[i].y = newy[i];
     }



    return true;

}


bool RenderThread::arc_length_four(MPolygon const *inipoly,
                                   MPolygon *apoly,
                                   double similarity, double TimeStep)
{

    if(!apoly)
        return false;

    remesh(apoly, 1e-8);


    if(apoly->PointNo() < 3)
    {

        ErrorCode=tr("Not a Polygon anymore");
        log<<"Not a Polygon anymore\n";
        return false;

    }

    int last = apoly->PointNo();

    double len[last];
    double tot_len = 0.0;
//    bool isRemeshNeeded = false;
    for(int i = 0; i<apoly->PointNo(); i++){
            double xs = apoly->points[(i+1)%last].x -
                        apoly->points[i].x;

            double ys = apoly->points[(i+1)%last].y -
                        apoly->points[i].y;

            len[i] = std::sqrt(xs*xs + ys*ys);
            tot_len += len[i];
     }



    compressed2D<double>                  A(last, last);
    A = 0.0;

        // Create inserter for matrix m

    mtl::matrix::inserter<compressed2D<double> > *ins;
    ins = new mtl::matrix::inserter<compressed2D<double> >(A);

    dense_vector<double> newx(last, 1.0), newy(last, 1.0),
                         bx(last), by(last);

    // Insert value in
    for(int i = 0; i < last; i++){
            double temp = 2.0*std::pow(len[i], 3.0/2.0);
            (*ins)[i][i] << 1.0+(1.0/temp + 2 * similarity) * TimeStep;
            (*ins)[i][(i+1)%last] << -TimeStep/temp;

            bx[i] = apoly->points[i].x +
                    2*TimeStep * similarity * inipoly->points[shuffle[i]].x;
            by[i] = apoly->points[i].y +
                    2*TimeStep * similarity * inipoly->points[shuffle[i]].y;
     }
     delete ins;

     pc::ilu_0<compressed2D<double> >        P(A);

        // Termination criterion: r < 1e-6 * b or N iterations
     basic_iteration<double>       iter1(bx, 500, 1.e-5);
     basic_iteration<double>       iter2(by, 500, 1.e-5);
        // Solve Ax == b with left preconditioner P

     bicgstab(A, newx, bx, P, iter1);
     bicgstab(A, newy, by, P, iter2);
     log<<iter1.resid()<<" "<<iter2.resid()<<"\n";


     if(!iter1.is_converged() ||
        !iter2.is_converged())
     {
         ErrorCode=tr("Not converged");
         log<<"Not converged\n";

//         log<<"segments: "<<cnt<<"\n";
         return false;
     }


     for(int i = 0; i < last; i++){
        apoly->points[i].x = newx[i];
        apoly->points[i].y = newy[i];
     }

     std::ofstream errp("poly_ini.txt");
     errp.precision(12);
     errp.setf(std::ios::fixed, std::ios::floatfield);
     for(int i = 0; i < apoly->points.size(); i++){
          errp<<apoly->points[i].x<<"\t"<<apoly->points[i].y<<"\n";
      }
    return true;

}

bool RenderThread::local_area(MPolygon const *inipoly,
                              MPolygon *apoly,
                              double similarity, double TimeStep)
{
    //==== compute local area first
    if(!apoly)
        return false;


    std::vector<int> newcard;


    apoly->reorder(1e-6, newcard);
    isKeepShuffle = true;
    for(int i = 0; i < apoly->PointNo(); i++)
        this->shuffle[i] = shuffle[newcard[i]];

    std::vector<double> Ai(apoly->PointNo(), 0.0);
    apoly->iArea(Ai, 1);
    int last = apoly->PointNo();

    std::vector<double> newx(last, 0.0);
    std::vector<double> newy(last, 0.0);

    if(isExplicit){

        log<<"Explict here"<<"\n";
        for(int i = 0 ; i < apoly->PointNo(); i++)
        {
           double DEx = (apoly->points[(i+last-2)%last].y - apoly->points[(i+last-1)%last].y) +
                        (apoly->points[(i+last+1)%last].y - apoly->points[(i+last-1)%last].y) +
                        (apoly->points[(i+last+1)%last].y - apoly->points[(i+last+2)%last].y)
                  +2*similarity*(apoly->points[i].x - inipoly->points[shuffle[i]].x);

           double DEy = -(apoly->points[(i+last-2)%last].x - apoly->points[(i+last-1)%last].x) -
                         (apoly->points[(i+last+1)%last].x - apoly->points[(i+last-1)%last].x)-
                         (apoly->points[(i+last+1)%last].x - apoly->points[(i+last+2)%last].x)
                  +2*similarity*(apoly->points[i].y - inipoly->points[shuffle[i]].y);

            newx[i] = apoly->points[i].x - TimeStep*DEx;
            newy[i] = apoly->points[i].y - TimeStep*DEy;
//            log<<inipoly->points[shuffle[i]].y<<"   "<<apoly->points[i].y<<"\n";
        }


    }else{
        log<<"Implict here"<<"\n";

    }

    log<<"new\n";
    for(int i = 0 ; i < last; i++)
    {
        apoly->points[i].x = newx[i];
        apoly->points[i].y = newy[i];
//        log<<newp.points[i].x<<"   "<<apoly->points[i].x<<"\n";
    }

    // kill time

    return true;

}

bool RenderThread::local_area_abs(MPolygon const *inipoly,
                                  MPolygon *apoly,
                                  double similarity, double TimeStep)
{
    //==== compute local area first
    if(!apoly)
        return false;


//    std::vector<int> newcard;


//    apoly->reorder(1e-6, newcard);
//    isKeepShuffle = true;
//    for(int i = 0; i < apoly->PointNo(); i++)
//        this->shuffle[i] = shuffle[newcard[i]];

    std::vector<double> Ai(apoly->PointNo(), 0.0);
    apoly->iArea(Ai, 1);

    // Ai stores the sign of Area of each triangle
    for(int i = 0; i < apoly->PointNo(); i++){
        if(fabs(Ai[i]) < 1e-12)
            Ai[i] = 0;
        else{
            if(Ai[i] > 0.0)
                Ai[i] = 1.0;
            if(Ai[i] < 0.0)
                Ai[i] = -1.0;
        }
    }
    int last = apoly->PointNo();

    std::vector<double> newx(last, 0.0);
    std::vector<double> newy(last, 0.0);

    if(isExplicit){

        log<<"Explict here"<<"\n";
        for(int i = 0 ; i < apoly->PointNo(); i++)
        {
           double DEx = Ai[(i+last-1)%last]*(apoly->points[(i+last-2)%last].y - apoly->points[(i+last-1)%last].y) +
                        Ai[(i+last)%last]*(apoly->points[(i+last+1)%last].y - apoly->points[(i+last-1)%last].y) +
                        Ai[(i+last+1)%last]*(apoly->points[(i+last+1)%last].y - apoly->points[(i+last+2)%last].y)
                  +2*similarity*(apoly->points[i].x - inipoly->points[shuffle[i]].x);

           double DEy = -Ai[(i+last-1)%last]*(apoly->points[(i+last-2)%last].x - apoly->points[(i+last-1)%last].x) -
                         Ai[(i+last)%last]*(apoly->points[(i+last+1)%last].x - apoly->points[(i+last-1)%last].x)-
                         Ai[(i+last+1)%last]*(apoly->points[(i+last+1)%last].x - apoly->points[(i+last+2)%last].x)
                  +2*similarity*(apoly->points[i].y - inipoly->points[shuffle[i]].y);

            newx[i] = apoly->points[i].x - TimeStep*DEx;
            newy[i] = apoly->points[i].y - TimeStep*DEy;
//            log<<inipoly->points[shuffle[i]].y<<"   "<<apoly->points[i].y<<"\n";
        }


    }else{
        log<<"Implict here"<<"\n";

    }

    log<<"new\n";
    for(int i = 0 ; i < last; i++)
    {
        apoly->points[i].x = newx[i];
        apoly->points[i].y = newy[i];
//        log<<newp.points[i].x<<"   "<<apoly->points[i].x<<"\n";
    }

    // kill time

    return true;

}

bool RenderThread::local_area_square(MPolygon const *inipoly,
                                     MPolygon *apoly,
                                     double similarity, double TimeStep)
{
    //==== compute local area first
    if(!apoly)
        return false;


    std::vector<int> newcard;


    apoly->reorder(1e-6, newcard);
    isKeepShuffle = true;
    for(int i = 0; i < apoly->PointNo(); i++)
        this->shuffle[i] = shuffle[newcard[i]];

    std::vector<double> Ai(apoly->PointNo(), 0.0);
    apoly->iArea(Ai, 1);
    int last = apoly->PointNo();

    std::vector<double> newx(last, 0.0);
    std::vector<double> newy(last, 0.0);

    if(isExplicit){

        log<<"Explict here"<<"\n";
        for(int i = 0 ; i < apoly->PointNo(); i++)
        {
           double DEx = 2*Ai[(i+last-1)%last]*(apoly->points[(i+last-2)%last].y - apoly->points[(i+last-1)%last].y) +
                  2*Ai[(i+last)%last]*(apoly->points[(i+last+1)%last].y - apoly->points[(i+last-1)%last].y) +
                  2*Ai[(i+last+1)%last]*(apoly->points[(i+last+1)%last].y - apoly->points[(i+last+2)%last].y)
                  +2*similarity*(apoly->points[i].x - inipoly->points[shuffle[i]].x);

           double DEy = -2*Ai[(i+last-1)%last]*(apoly->points[(i+last-2)%last].x - apoly->points[(i+last-1)%last].x) -
                  2*Ai[(i+last)%last]*(apoly->points[(i+last+1)%last].x - apoly->points[(i+last-1)%last].x)-
                  2*Ai[(i+last+1)%last]*(apoly->points[(i+last+1)%last].x - apoly->points[(i+last+2)%last].x)
                  +2*similarity*(apoly->points[i].y - inipoly->points[shuffle[i]].y);

            newx[i] = apoly->points[i].x - TimeStep*DEx;
            newy[i] = apoly->points[i].y - TimeStep*DEy;
//            log<<inipoly->points[shuffle[i]].y<<"   "<<apoly->points[i].y<<"\n";
        }


    }else{
        log<<"Implict here"<<"\n";

    }

    log<<"new\n";
    for(int i = 0 ; i < last; i++)
    {
        apoly->points[i].x = newx[i];
        apoly->points[i].y = newy[i];
//        log<<newp.points[i].x<<"   "<<apoly->points[i].x<<"\n";
    }

    // kill time

    return true;

}

bool RenderThread::multiple_local_area(MPolygon const *inipoly,
                                       MPolygon *apoly,
                                       int jump,
                                       double similarity, double TimeStep)
{
    //==== compute local area first

    if(!apoly)
        return false;


    if((jump < 1) == true)
        return false;


    std::vector<int> newcard;

    apoly->reorder(1e-6, newcard);
    isKeepShuffle = true;
    for(int i = 0; i < apoly->PointNo(); i++)
        this->shuffle[i] = shuffle[newcard[i]];


    int last = apoly->PointNo();

    if(jump > (last / 2 - 1))
        return false;

    std::vector<std::vector<double> > Ai(jump, std::vector<double>(apoly->PointNo(), 0.0));
    for(int j = 1; j <= jump; j++)
        apoly->iArea(Ai[j-1] , j);

    std::vector<double> newx(last, 0.0);
    std::vector<double> newy(last, 0.0);

    if(isExplicit){

        log<<"Explict here"<<"\n";

        for(int i = 0 ; i < apoly->PointNo(); i++)
        {
           double DEx = 0.0;

           double DEy = 0.0;

           for(int j = 1; j <= jump; j++)
           {
               int i0 = (i+last-2*j)%last;
               int i1 = (i+last-j)%last;
               int i2 = (i+last)%last;
               int i3 = (i+last+j)%last;
               int i4 = (i+last+2*j)%last;

               DEx += 2*Ai[j-1][i1]*(apoly->points[i0].y - apoly->points[i1].y) +
                      2*Ai[j-1][i2]*(apoly->points[i3].y - apoly->points[i1].y) +
                      2*Ai[j-1][i3]*(apoly->points[i3].y - apoly->points[i4].y);

               DEy +=-2*Ai[j-1][i1]*(apoly->points[i0].x - apoly->points[i1].x) -
                      2*Ai[j-1][i2]*(apoly->points[i3].x - apoly->points[i1].x) -
                      2*Ai[j-1][i3]*(apoly->points[i3].x - apoly->points[i4].x);
           }

           DEx += 2*similarity*(apoly->points[i].x - inipoly->points[shuffle[i]].x);
           DEy += 2*similarity*(apoly->points[i].y - inipoly->points[shuffle[i]].y);

           newx[i] = apoly->points[i].x - TimeStep*DEx;
           newy[i] = apoly->points[i].y - TimeStep*DEy;

        }


//        log<<inipoly->points[shuffle[i]].y<<"   "<<apoly->points[i].y<<"\n";

    }else{
        log<<"Implict here"<<"\n";

    }

    log<<"new\n";
    for(int i = 0 ; i < last; i++)
    {
        apoly->points[i].x = newx[i];
        apoly->points[i].y = newy[i];
//        log<<newp.points[i].x<<"   "<<apoly->points[i].x<<"\n";
    }



    return true;

}

bool RenderThread::bilateral_normal(const MPolygon *inipoly,
                                    MPolygon *apoly,
                                    double similarity,
                                    double TimeStep,
                                    double VarVertex,
                                    double VarNormal)
{
    bilateral_normal_filter filter(apoly);



    bool status;


    if(status = filter.NormalUpdate(VarVertex, VarNormal, 2)){
//    if(1){
        if(filter.VertexUpdate(5)){
            for(int i = 0; i<filter.polygon->PointNo(); i++)
            {
                apoly->points[i].x = filter.polygon->points[i].x;
                apoly->points[i].y = filter.polygon->points[i].y;
            }
            log<<"normals size: "<<filter.polygon->normals.size()<<"\n";
            log<<"normals x: "<<filter.polygon->normals[0].x<<"\n";
            log<<"normals y: "<<filter.polygon->normals[0].y<<"\n";
            log<<"normals len: "<<filter.polygon->normals[0].length()<<"\n";
            log<<"status: "<<status<<"\n";
            return true;
        }else{
            return false;
        }
    }else{
        log<<"status: "<<status<<"\n";
        return false;
    }

}


bool  RenderThread::cos_pi(const MPolygon *inipoly,
                          MPolygon *apoly,
                          double similarity,
                          double TimeStep)
{
    //==== compute local area first
    if(!apoly)
        return false;

    int last = apoly->PointNo();

    std::vector<double> newx(last, 0.0);
    std::vector<double> newy(last, 0.0);

    std::ofstream fout("residual.txt");

    double eps = 1e-8;

    if(isExplicit){

        log<<"Explict here"<<"\n";
        for(int i = 0 ; i < apoly->PointNo(); i++)
        {
            //p(i-2)
            Point2d p1(apoly->points[(i-2 + last)%last].x,
                       apoly->points[(i-2 + last)%last].y);
            //p(i-1)
            Point2d p2(apoly->points[(i-1 + last)%last].x,
                       apoly->points[(i-1 + last)%last].y);
            //p(i)
            Point2d p3(apoly->points[(i-0 + last)%last].x,
                       apoly->points[(i-0 + last)%last].y);
            //p(i+1)
            Point2d p4(apoly->points[(i+1 + last)%last].x,
                       apoly->points[(i+1 + last)%last].y);
            //p(i+2)
            Point2d p5(apoly->points[(i+2 + last)%last].x,
                       apoly->points[(i+2 + last)%last].y);

            Vector2d d1(p3, p1);
            Vector2d d1b(p2, p1);
            Vector2d d1f(p2, p3);
            Vector2d d2(p4, p2);
            Vector2d d2b(p3, p2);
            Vector2d d2f(p3, p4);
            Vector2d d3(p5, p3);
            Vector2d d3b(p4, p3);
            Vector2d d3f(p4, p5);

            double r1 = d1.length()*d1.length() - (d1b.length()+d1f.length())*(d1b.length()+d1f.length());
            double r2 = d2.length()*d2.length() - (d2b.length()+d2f.length())*(d2b.length()+d2f.length());
            double r3 = d3.length()*d3.length() - (d3b.length()+d3f.length())*(d3b.length()+d3f.length());

            double d1x = -d1.x / (d1.length()+eps);
            double d1y = -d1.y / (d1.length()+eps) ;

            double d1fx = d1f.x / (d1f.length()+eps);
            double d1fy = d1f.y / (d1f.length()+eps);


            double d2bx = -d2b.x / (d2b.length()+eps);
            double d2by = -d2b.y / (d2b.length()+eps);

            double d2fx = -d2f.x / (d2f.length()+eps);
            double d2fy = -d2f.y / (d2f.length()+eps);

            double d3x = d3.x / (d3.length()+eps);
            double d3y = d3.y / (d3.length()+eps);

            double d3bx = d3b.x / (d3b.length()+eps);
            double d3by = d3b.y / (d3b.length()+eps);

            double r1x = 2.0*d1.length()*d1x - 2.0*(d1f.length() + d1b.length())*(d1fx);
            double r1y = 2.0*d1.length()*d1y - 2.0*(d1f.length() + d1b.length())*(d1fy);

            double r2x =  - 2.0*(d2f.length() + d2b.length())*(d2fx + d2bx);
            double r2y =  - 2.0*(d2f.length() + d2b.length())*(d2fy + d2by);

            double r3x = 2.0*d3.length()*d3x - 2.0*(d3f.length() + d3b.length())*(d3bx);
            double r3y = 2.0*d3.length()*d3y - 2.0*(d3f.length() + d3b.length())*(d3by);

            double t1 = d1b.length()*d1f.length();
            double t2 = d2b.length()*d2f.length();
            double t3 = d3b.length()*d3f.length();

            double sr1 = 0.0;
            if(fabs(r1)<1e-10)
                sr1 = 0.0;
            else if(r1 > 0.0)
                sr1 = 1.0;
            else if(r1 < 0.0)
                sr1 = -1.0;

            double sr2 = 0.0;

            if(fabs(r2)<1e-10)
                sr2 = 0.0;
            else if(r2 > 0.0)
                sr2 = 1.0;
            else if(r2 < 0.0)
                sr2 = -1.0;

            double sr3 = 0.0;
            if(fabs(r3)<1e-10)
                sr3 = 0.0;
            else if(r3 > 0.0)
                sr3 = 1.0;
            else if(r3 < 0.0)
                sr3 = -1.0;

            double DEx = 2.0*sr1*r1x / (t1+ 0.00001)+ 2.0*sr2*r2x / (t2+0.00001) + 2.0*sr3*r3x / (t3+0.00001)-
                         d1b.length()*d1fx*fabs(r1) / (t1*t1+ eps) -
                         (d2b.length()*d2fx + d2f.length()*d2bx)*fabs(r2) / (t2*t2+ eps) -
                         d3bx*d3f.length()*fabs(r3)/(t3*t3 + eps);
            double DEy = 2.0*sr1*r1y / (t1+0.00001)+ 2.0*sr2*r2y / (t2+0.00001) + 2.0*sr3*r3y / (t3+0.00001)-
                         d1b.length()*d1fy*fabs(r1) / (t1*t1+ eps) -
                         (d2b.length()*d2fy + d2f.length()*d2by)*fabs(r2) / (t2*t2+ eps) -
                         d3by*d3f.length()*fabs(r3)/(t3*t3 + eps);


            newx[i] = apoly->points[i].x - TimeStep*DEx;
            newy[i] = apoly->points[i].y - TimeStep*DEy;


            fout<<i<<" DEx: "<<DEx<<"; "<<r1<<", "<<r2<<", "<<r3<<"\n";
            fout<<i<<" DEx: "<<r1x<<", "<<r2x<<", "<<r3x<<"\n\n";
//            log<<inipoly->points[shuffle[i]].y<<"   "<<apoly->points[i].y<<"\n";
        }


    }else{
        compressed2D<double>                  A(last, last);
        A = 0.0;

        // Create inserter for matrix m
        mtl::matrix::inserter<compressed2D<double> > *ins;
        ins = new mtl::matrix::inserter<compressed2D<double> >(A);

        dense_vector<double> newxx(last, 1.0), newyy(last, 1.0),
                             bx(last), by(last);

        // Insert value in
        std::vector<Vector2d> vece(last,Vector2d());
        std::vector<double> len(last,0.0);
        for(int i = 0; i<last; i++){
            Point2d p1(apoly->points[(i + last)%last].x,
                       apoly->points[(i + last)%last].y);
            //p(i-1)
            Point2d p2(apoly->points[(i+1 + last)%last].x,
                       apoly->points[(i+1 + last)%last].y);

            vece[i] = Vector2d(p1,p2);
            len[i] = vece[i].length();
        }


        for(int i = 0; i < last; i++){
                double k1 = vece[(i-2+last)%last].inner_product(vece[(i-1+last)%last]) /
                             (len[(i-2+last)%last]*len[(i-1+last)%last] + eps);
                double k2 = vece[(i-1+last)%last].inner_product(vece[(i+last)%last]) /
                             (len[(i-1+last)%last]*len[(i+last)%last] + eps);
                double k3 = vece[(i+last)%last].inner_product(vece[(i+1+last)%last]) /
                             (len[(i+last)%last]*len[(i+1+last)%last] + eps);

                double a1 = 1.0 / (len[(i-2+last)%last]*len[(i-1+last)%last] + eps);

                double a2 = -(1.0 / (len[(i-2+last)%last] * len[(i-1+last)%last] + eps)+
                              1.0 / (len[(i-1+last)%last] * len[(i+last)%last] + eps)+
                             (k1 + k2) / (len[(i-1+last)%last] * len[(i-1+last)%last] + eps));

                double a3 = (2.0/(len[(i-1+last)%last] * len[(i+last)%last] + eps) +
                             (k1 + k2) / (len[(i-1+last)%last] * len[(i-1+last)%last] + eps)+
                             (k2 + k3) / (len[(i+last)%last] * len[(i+last)%last] + eps));

                double a4 = -(1.0 / (len[(i+last)%last] * len[(i-1+last)%last] + eps)+
                              1.0 / (len[(i+1+last)%last] * len[(i+last)%last] + eps)+
                             (k2 + k3) / (len[(i+last)%last] * len[(i+last)%last] + eps));

                double a5 = 1.0 / (len[(i+last)%last] * len[(i+1+last)%last] + eps);

                (*ins)[i][(i-2+last)%last] << TimeStep*a1;
                (*ins)[i][(i-1+last)%last] << TimeStep*a2;
                (*ins)[i][i] << 1.0+(a3 + 2 * similarity) * TimeStep;
                (*ins)[i][(i+1)%last] << TimeStep*a4;
                (*ins)[i][(i+2)%last] << TimeStep*a5;

                bx[i] = apoly->points[i].x +
                        2*TimeStep * similarity * inipoly->points[i].x;
                by[i] = apoly->points[i].y +
                        2*TimeStep * similarity * inipoly->points[i].y;
         }
         delete ins;

         pc::ilu_0<compressed2D<double> >        P(A);

            // Termination criterion: r < 1e-6 * b or N iterations
         basic_iteration<double>       iter1(bx, 500, 1.e-5);
         basic_iteration<double>       iter2(by, 500, 1.e-5);
            // Solve Ax == b with left preconditioner P

         bicgstab(A, newxx, bx, P, iter1);
         bicgstab(A, newyy, by, P, iter2);
         log<<iter1.resid()<<" "<<iter2.resid()<<"\n";


         if(!iter1.is_converged() ||
            !iter2.is_converged())
         {
             ErrorCode=tr("Not converged");
             log<<"Not converged\n";

    //         log<<"segments: "<<cnt<<"\n";
             return false;
         }

         for(int i = 0 ; i < last; i++)
         {
             newx[i] = newxx[i];
             newy[i] = newyy[i];
     //        log<<newp.points[i].x<<"   "<<apoly->points[i].x<<"\n";
         }

    }

    log<<"new\n";
//    std::vector<Vector2d> inc;
//    length_diff(apoly,inc);
    for(int i = 0 ; i < last; i++)
    {
        apoly->points[i].x = newx[i];
        apoly->points[i].y = newy[i];
//        log<<newp.points[i].x<<"   "<<apoly->points[i].x<<"\n";
    }

    // kill time

    return true;

}

bool RenderThread::turning_angle(MPolygon const *inipoly,
                                 MPolygon *apoly,
                                 double similarity,
                                 double TimeStep){

    if(!apoly)
        return false;

    double eps = 1e-8;
//    std::vector<MPolygon> newPolygons = polys;

//    for(int k = 0; k < polys.size(); k++){
        int last = apoly->PointNo();

        std::vector<double> newx(last, 0.0);
        std::vector<double> newy(last, 0.0);

        if(last < 3) return true;
//        if(fabs(apoly->Area()) < 1e-12) retu;
        if(isExplicit){
            for(int i = 0; i < last; i++){
                //p(i-2)
                Point2d p1(apoly->points[(i-2 + last)%last].x,
                           apoly->points[(i-2 + last)%last].y);
                //p(i-1)
                Point2d p2(apoly->points[(i-1 + last)%last].x,
                           apoly->points[(i-1 + last)%last].y);
                //p(i)
                Point2d p3(apoly->points[(i-0 + last)%last].x,
                           apoly->points[(i-0 + last)%last].y);
                //p(i+1)
                Point2d p4(apoly->points[(i+1 + last)%last].x,
                           apoly->points[(i+1 + last)%last].y);
                //p(i+2)
                Point2d p5(apoly->points[(i+2 + last)%last].x,
                           apoly->points[(i+2 + last)%last].y);

                Vector2d ve1(p1, p2);
                Vector2d ve2(p2, p3);
                Vector2d ve3(p3, p4);
                Vector2d ve4(p4, p5);

                double e1 = ve1.length();
                double e2 = ve2.length();
                double e3 = ve3.length();
                double e4 = ve4.length();

                double eta1 = ve1.inner_product(ve2)/e1/e2;
                double eta2 = ve2.inner_product(ve3)/e2/e3;
                double eta3 = ve3.inner_product(ve4)/e3/e4;

                Vector2d inc1 = (ve1 - eta1*e1/e2*ve2)/e1/e2;

                Vector2d inc2 = (ve3 - ve2 - eta2*(e3/e2*ve2-e2/e3*ve3))/e1/e2;

                Vector2d inc3 = (-1.0*ve4 + eta3*e4/e3*ve3)/e1/e2;


                inc1 *= (-1.0/(std::sqrt(1.0-eta1*eta1)+eps));
                inc1 *= (std::acos(eta1)/(std::sqrt(std::acos(eta1)+eps)));

                inc2 *= (-1.0/((std::sqrt(1.0-eta2*eta2)+eps)));
                inc2 *= (std::acos(eta2)/(std::sqrt(std::acos(eta2)+eps)));

                inc3 *= (-1.0/((std::sqrt(1.0-eta3*eta3)+eps)));

                inc3 *= (std::acos(eta3)/(std::sqrt(std::acos(eta3)+eps)));


                if(fabs(eta1-1.0)<eps)
                {inc1.x = 0.0;inc1.y = 0.0;}
                if(fabs(eta2-1.0)<eps)
                {inc2.x = 0.0;inc2.y = 0.0;}
                if(fabs(eta3-1.0)<eps)
                {inc3.x = 0.0;inc3.y = 0.0;}
                newx[i] = apoly->points[i].x - TimeStep*(inc1.x + inc2.x + inc3.x);
                newy[i] = apoly->points[i].y - TimeStep*(inc1.y + inc2.y + inc3.y);

            }
        }else{

        }

        for(int i = 0 ; i < last; i++)
        {

            apoly->points[i].x = newx[i];
            apoly->points[i].y = newy[i];
        }

    return true;
}

bool RenderThread::total_area(MPolygon const *inipoly,
                              std::vector<MPolygon> &polys,
                              double similarity, double TimeStep)
{
    if(polys.empty())
        return false;

    std::vector<MPolygon> newPolygons = polys;

    int orientation;
    std::vector<std::vector<int> > abc;
    for(int k = 0; k < polys.size(); k++){
        int last = polys[k].PointNo();
        if(last < 3) continue;
        if(fabs(polys[k].Area()) < 1e-12) continue;

        if(polys[k].Area() > 0.0)
            orientation = 1;
        else
            orientation = -1;

        std::vector<double> speedx(last, 0.0);
        std::vector<double> speedy(last, 0.0);
        std::vector<double> VertexTime(last,0.0);

        if(isExplicit){

            log<<"Explict here"<<"\n";

            for(int i = 0 ; i < polys[k].PointNo(); i++)
            {
                double DEx = 0.5*(polys[k].points[(i+last+1)%last].y - polys[k].points[(i+last-1)%last].y) + 0.0;
                       // 2*similarity*(apoly->points[i].x - inipoly->points[shuffle[i]].x);

                double DEy = -0.5*(polys[k].points[(i+last+1)%last].x - polys[k].points[(i+last-1)%last].x)+ 0.0;
                      //  2*similarity*(apoly->points[i].y - inipoly->points[shuffle[i]].y);

                double normlen = sqrt(DEx*DEx + DEy*DEy + 1e-12);
                normlen = 1.0;
                speedx[i] = -orientation*DEx / normlen;
                speedy[i] = -orientation*DEy / normlen;
//            log<<inipoly->points[shuffle[i]].y<<"   "<<apoly->points[i].y<<"\n";
            }


        }else{
            log<<"Implict here"<<"\n";

        }
//For each polygon, determine a time step without collision
        double SmallestTime = TimeStep;
        double tempTime;
        int token = -1;

        int segment = -1;
        for(int i = 0; i < polys[k].PointNo(); i++){

            VertexTime[i] = TimeStep;
            Vector2d v0;
            v0.x = speedx[i];
            v0.y = speedy[i];
            Point2d p0 = polys[k].points[i];
            for(int j = 0; j < polys[k].PointNo(); j++){
                if(j==i || i==(j+1)%last)
                    continue;
                else{
                    Point2d p1 = polys[k].points[j];
                    Point2d p2 = polys[k].points[(j+1)%last];
                    Vector2d v1;
                    v1.x = speedx[j];
                    v1.y = speedy[j];
                    Vector2d v2;
                    v2.x = speedx[(j+1)%last];
                    v2.y = speedy[(j+1)%last];

                    double s1,s2;
                    tempTime = CollisionTime(p0, p1, p2, v0, v1, v2, s1, s2);

                    if(tempTime > -0.5){
                        VertexTime[i] = (tempTime<VertexTime[i] ? tempTime:VertexTime[i]);
                        if(SmallestTime > tempTime){
                            SmallestTime = tempTime;
                            token = i;
                            segment = j;
                        }
                    }
                 }
              }
          }


        log<<"new\n";
        for(int i = 0 ; i < last; i++)
        {
            polys[k].points[i].x += SmallestTime * speedx[i];
            polys[k].points[i].y += SmallestTime * speedy[i];
        }


    // split one polygon
        if(token != -1){

            int firstSize = 0;
            int secondSize = 0;

            secondSize = (segment-token+last)%last + 1;
            firstSize = last + 1 - secondSize;

            MPolygon newpoly2;
            newPolygons[k].points.resize(firstSize,Point2d());
            for(int pt = 0; pt < firstSize; pt++)
            {
                int old = (pt + segment+1) % last;
                newPolygons[k].points[pt] = polys[k].points[old];
            }

            for(int pt = 0; pt < secondSize; pt++)
            {
                int old = (pt + token) % last;
                newpoly2.points.push_back(polys[k].points[old]);
            }

            newPolygons.push_back(newpoly2);
//++++ Update existing Joints and add a new joint
            for(int ji = 0; ji < this->Joints.size(); ji ++){
                if(Joints[ji][0] == k){
                    if((Joints[ji][1] - token + last) % last < secondSize){
                        Joints[ji][1] = (Joints[ji][1] - token + last) % last;
                        Joints[ji][0] = newPolygons.size()-1;
                    }else{
                        Joints[ji][1] = (Joints[ji][1] - segment - 1 + last) % last;
                        Joints[ji][0] = k;
                    }
                }
                if(Joints[ji][2] == k){
                    if((Joints[ji][3] - token + last) % last < secondSize){
                        Joints[ji][3] = (Joints[ji][3] - token + last) % last;
                        Joints[ji][2] = newPolygons.size()-1;
                    }else{
                        Joints[ji][3] = (Joints[ji][3] - segment - 1 + last) % last;
                        Joints[ji][2] = k;
                    }
                }
            }
            std::vector<int> joint(4,0);
            joint[0] = k;
            joint[1] = firstSize-1;
            joint[2] = newPolygons.size()-1;
            joint[3] = 0;
            this->Joints.push_back(joint);
            continue;
        }
  }

    if(newPolygons.size() > polys.size()){
        polys.clear();
        for(int k = 0; k < newPolygons.size(); k++)
            polys.push_back(newPolygons[k]);
    }
    for(int ji = 0; ji < this->Joints.size(); ji++){
        log << Joints[ji][0]<<" "<<Joints[ji][1]<<" "<<Joints[ji][2]<<" "<<Joints[ji][3]<<"\n";
        abc.push_back(this->Joints[ji]);
    }


//    abc.push_back(this->Joints[0]);
    return true;

}

bool RenderThread::discrete_curvature_normal(MPolygon const *inipoly,
                               MPolygon *apoly,
                               double similarity,
                               double TimeStep){
    if(!apoly)
        return false;

    double eps = 1e-8;
    double alpha = 1.0;
//    std::vector<MPolygon> newPolygons = polys;

//    for(int k = 0; k < polys.size(); k++){
        int last = apoly->PointNo();

        std::vector<double> newx(last, 0.0);
        std::vector<double> newy(last, 0.0);

        if(last < 3) return true;
//        if(fabs(apoly->Area()) < 1e-12) retu;
        if(isExplicit){
            for(int i = 0; i < last; i++){
                //p(i-2)
                Point2d p1(apoly->points[(i-2 + last)%last].x,
                           apoly->points[(i-2 + last)%last].y);
                //p(i-1)
                Point2d p2(apoly->points[(i-1 + last)%last].x,
                           apoly->points[(i-1 + last)%last].y);
                //p(i)
                Point2d p3(apoly->points[(i-0 + last)%last].x,
                           apoly->points[(i-0 + last)%last].y);
                //p(i+1)
                Point2d p4(apoly->points[(i+1 + last)%last].x,
                           apoly->points[(i+1 + last)%last].y);
                //p(i+2)
                Point2d p5(apoly->points[(i+2 + last)%last].x,
                           apoly->points[(i+2 + last)%last].y);

                Vector2d ve1(p1, p2);//P_{i-2}->P_{i-1}
                Vector2d ve2(p2, p3);//P_{i-1}->P_{i}
                Vector2d ve3(p3, p4);//P_{i}->P_{i+1}
                Vector2d ve4(p4, p5);//P_{i+1}->P_{i+2}

                double e1 = ve1.length();
                double e2 = ve2.length();
                double e3 = ve3.length();
                double e4 = ve4.length();

                Vector2d eta1 = ve2/(e2+eps) - ve1/(e1+eps);
                Vector2d eta2 = ve3/(e3+eps) - ve2/(e2+eps);
                Vector2d eta3 = ve4/(e4+eps) - ve3/(e3+eps);

                double d1 = alpha*std::pow((eta1.length()*eta1.length()+eps), alpha/2.0 - 1.0);
                double d2 = alpha*std::pow((eta2.length()*eta2.length()+eps), alpha/2.0 - 1.0);
                double d3 = alpha*std::pow((eta3.length()*eta3.length()+eps), alpha/2.0 - 1.0);

                Vector2d inc1x;
                inc1x.x =(ve2.y)*(ve2.y)/e2/e2/e2;
                inc1x.y = -(ve2.x)*(ve2.y)/e2/e2/e2;

                Vector2d inc2x;
                inc2x.x = -((ve3.y)*(ve3.y)/e3/e3/e3+(ve2.y)*(ve2.y)/e2/e2/e2);
                inc2x.y = (ve3.x)*(ve3.y)/e3/e3/e3+(ve2.x)*(ve2.y)/e2/e2/e2;

                Vector2d inc3x;
                inc3x.x = (ve3.y)*(ve3.y)/e3/e3/e3;
                inc3x.y =-(ve3.x)*(ve3.y)/e3/e3/e3;

                Vector2d inc1y;
                inc1y.x = -(ve2.x)*(ve2.y)/e2/e2/e2;
                inc1y.y = (ve2.x)*(ve2.x)/e2/e2/e2;

                Vector2d inc2y;
                inc2y.x = (ve3.x)*(ve3.y)/e3/e3/e3+(ve2.x)*(ve2.y)/e2/e2/e2;
                inc2y.y = -((ve3.x)*(ve3.x)/e3/e3/e3+(ve2.x)*(ve2.x)/e2/e2/e2);

                Vector2d inc3y;
                inc3y.x = -(ve3.x)*(ve3.y)/e3/e3/e3,
                inc3y.y = (ve3.x)*(ve3.x)/e3/e3/e3;

                double d1x = d1*eta1.inner_product(inc1x);
                double d2x = d2*eta2.inner_product(inc2x);
                double d3x = d3*eta3.inner_product(inc3x);

                double d1y = d1*eta1.inner_product(inc1y);
                double d2y = d2*eta2.inner_product(inc2y);
                double d3y = d3*eta3.inner_product(inc3y);

                newx[i] = apoly->points[i].x - TimeStep*(d1x + d2x + d3x + 2*similarity*(apoly->points[i].x - inipoly->points[i].y));
                newy[i] = apoly->points[i].y - TimeStep*(d1y + d2y + d3y + 2*similarity*(apoly->points[i].y - inipoly->points[i].y));

            }
        }else{

        }

        for(int i = 0 ; i < last; i++)
        {

            apoly->points[i].x = newx[i];
            apoly->points[i].y = newy[i];
        }

    return true;
}
